#ifndef PATH_PLANNER_H
#define PATH_PLANNER_H

#include <libplayerc++/playerc++.h>
#include "path_planning/avl.h"
#include "path_planning/topological_map.h"
#include <limits.h>
#include <iostream>
#include <math.h>

using namespace std;
using namespace PlayerCc;

using namespace PlayerCc;

class WaypointList{

public:
	WaypointList();

	~WaypointList();

	bool Empty() const;

	player_point_2d* Head() const;

	player_point_2d* Next();

	player_point_2d* Previous();

	player_point_2d* Add(double x_coordinate, double y_coordinate);

	player_point_2d* Remove(player_point_2d* point);

	void Reset();

private:
	struct Node{
		player_point_2d *point;
		Node() : point(NULL), next(NULL), previous(NULL){};
		Node *next;
		Node *previous;
	};
	Node *first_;
	Node *last_;
	Node *current_;

	bool end_of_list_;
};

//
// Robot Planner
//
class PathPlanner{

public:

	PathPlanner(string grid_map_path_file, string topological_map_file_descriptor);

	//
	// Calculate the route given a origin and a destination coordinate.
	//
	// return the route requested or NULL if the goal is not reachable
	//
	WaypointList* RequestWaypoints(player_point_2d origin, player_point_2d goal);

	//
	// Calculate an alternative route
	//
	// return the alternative route requested or NULL if the goal is not reachable
	//
	WaypointList* RequestAlternativeRoute(player_point_2d current_position, player_point_2d goal_position,
											player_point_2d previous_node_position, player_point_2d next_node_position );


private:
	//
	// If the adjacent vertex or node is on the open list already, this operation is called to see if the current path to
	// that adjacent node is better, using G cost as the measure. A lower G cost means that this is a better path.
	// If so, change the parent of the adjacent to the current node, and recalculate the G and F scores of
	// the adjacent cell. As we are getting the open list sorted by F score, we may need to resort the list to account
	// for the change.
	//
	void SearchForShortcut(Vertex* current, double edge_cost, Vertex* adjacent);

	//
	// Obtain F,G,H scores
	// Where F=G+H
	// H=>(Horizontal+vertical) distance, in cells unity,  from cell being analyzed to cell destination
	// G=> This is used to express the cost of the cell being analysed, considering the origin point
	//
	void CalculateFGH(Vertex* adjacent, double edge_cost, Vertex* destination);

	//
	// Parse all adjacent vertices. Taking actions according to A* search algorithm.
	// For each adjacent:
	// - If it is on the closed list, ignore it. Otherwise do the following.
	// - If it isnt on the open list, add it to the open list. Make the current vertex the parent of
	// this vertex. Record the F, G, and H costs of the adjacent.
	// - If it is on the open list already, check to see if this path to that cell is better, using
	// G cost as the measure. A lower G cost means that this is a better path. If so, change the parent
	// of the adjacent to the current vertex, and recalculate the G and F scores of the cell. As we are keeping
	// our open list (actually a tree) sorted by F score, we may need to resort the list to account for the change.
	//
	void ParseAdjacents(int current_id);

	void Clean();

private:
	PlannerMap* map_;

	Vertex *start_;
	Vertex *target_;

	AvlTree *open_avl_tree_;
	AvlTree *closed_avl_tree_;

	bool map_retrieved_;
};

#endif


